#
# Infrared obstacle avoidance sensor
#
import time
from .motors_gpiozero import *
from gpiozero import  DigitalInputDevice
from gpiozero.pins.pigpio import PiGPIOFactory



class Infrared_Avoidance:
    AVOIDANCE_ON = 1
    AVOIDANCE_OFF = 0

    def __init__(self, left_sensor, right_sensor):
        self.Left=DigitalInputDevice(pin=left_sensor,pull_up=False,pin_factory=PiGPIOFactory())
        self.Right=DigitalInputDevice(pin=right_sensor,pull_up=False,pin_factory=PiGPIOFactory())
        
        self.Detected_Left_Barrier = False
        self.Detected_Right_Barrier = False
        self.Detected_Barrier = False
        self.avoidance = False

    def detected_barrier(self):
        if (self.Left.value == 0):  # 当检测到障碍物时，输出低电平信号
            self.Detected_Left_Barrier = True
            self.Detected_Barrier = True
        else:
            self.Detected_Left_Barrier = False

        if (self.Right.value == 0):  # 当检测到障碍物时，输出低电平信号
            self.Detected_Right_Barrier = True
            self.Detected_Barrier = True
        else:
            self.Detected_Right_Barrier = False
        if (self.Detected_Right_Barrier == False and self.Detected_Left_Barrier == False):
            self.Detected_Left_Barrier = False
            self.Detected_Right_Barrier = False
            self.Detected_Barrier = False

    def action(self, cmd_to_ir):
        if (cmd_to_ir == Infrared_Avoidance.AVOIDANCE_ON):
            self.avoidance = True
        else:
            if (cmd_to_ir == Infrared_Avoidance.AVOIDANCE_OFF):
                self.avoidance = False

    def worker(self, robot):
        """thread worker function"""
        while True:
            time.sleep(0.01)
            self.detected_barrier()
            if (self.avoidance):
                if (robot.MOTOR_STATE == RobotCar.FORWARD or robot.MOTOR_STATE == RobotCar.LEFT or robot.MOTOR_STATE == RobotCar.RIGHT):
                    if (self.Detected_Barrier):
                        robot.action(RobotCar.STOP)


if __name__ == '__main__':     # Program start from here
    IRroas = Infrared_Avoidance(22, 16)
    try:
        while True:
            IRroas.detected_barrier()
            if (IRroas.Detected_Left_Barrier):
                print("Detected Left Barrier!")
            else:
                print("****Left Nothing!******")

            if (IRroas.Detected_Right_Barrier):
                print("Detected Right Barrier!")
            else:
                print("****Right Nothing!******")
    except KeyboardInterrupt:
        IRroas.destroy()
